
#include "bledecoder.hpp"
#define _USE_MATH_DEFINES
#include <math.h>

#define RB(l) rb_buf[(rb_head+(l))%RB_SIZE]
#define Q(l) Quantize(l)
#define RB_SIZE 1000

bledecoder::bledecoder() :
	samples(0),
	chan(39),
	skipSamples(50),
	srate(2)
{
	rb_buf = (int16_t *)malloc(RB_SIZE * 2);
	atan_lut_init();
}

bledecoder::~bledecoder()
{
	if(rb_buf)
		free(rb_buf);
}

void bledecoder::set_access_addr(uint64_t _access_addr) {
	access_addr = _access_addr;
}

void bledecoder::set_channel(uint8_t _channel) {
	chan = _channel;
}

long long bledecoder::get_channel_freq(int channel_number) {

	long long freq_hz;
	if (channel_number == 37) {
		freq_hz = 2402000000;
	}
	else if (channel_number == 38) {
		freq_hz = 2426000000;
	}
	else if (channel_number == 39) {
		freq_hz = 2480000000;
	}
	else if (channel_number >= 0 && channel_number <= 10) {
		freq_hz = 2404000000 + channel_number * 2000000;
	}
	else if (channel_number >= 11 && channel_number <= 36) {
		freq_hz = 2428000000 + (channel_number - 11) * 2000000;
	}
	else {
		freq_hz = 0;
	}
	return(freq_hz);

}

void bledecoder::set_callback(std::function<void(lell_packet)> _callback) {
	callback = _callback;
}

int bledecoder::atan_lut_init()
{
	int i = 0;

	atan_lut = new int[atan_lut_size];

	for (i = 0; i < atan_lut_size; i++) {
		atan_lut[i] = (int)(atan((double)i / (1 << atan_lut_coef)) / 3.14159 * (1 << 14));
	}

	return 0;
}

void bledecoder::multiply(int ar, int aj, int br, int bj, int *cr, int *cj)
{
        *cr = ar*br - aj*bj;
        *cj = aj*br + ar*bj;
}


int bledecoder::polar_disc_lut(int ar, int aj, int br, int bj)
{
	int cr, cj, x, x_abs;

	multiply(ar, aj, br, -bj, &cr, &cj);

	/* special cases */
	if (cr == 0 || cj == 0) {
		if (cr == 0 && cj == 0)
		{
			return 0;
		}
		if (cr == 0 && cj > 0)
		{
			return 1 << 13;
		}
		if (cr == 0 && cj < 0)
		{
			return -(1 << 13);
		}
		if (cj == 0 && cr > 0)
		{
			return 0;
		}
		if (cj == 0 && cr < 0)
		{
			return 1 << 14;
		}
	}

	/* real range -32768 - 32768 use 64x range -> absolute maximum: 2097152 */
	x = (cj << atan_lut_coef) / cr;
	x_abs = abs(x);

	if (x_abs >= atan_lut_size) {
		/* we can use linear range, but it is not necessary */
		return (cj > 0) ? 1 << 13 : -1 << 13;
	}

	if (x > 0) {
		return (cj > 0) ? atan_lut[x] : atan_lut[x] - (1 << 14);
	}
	else {
		return (cj > 0) ? (1 << 14) - atan_lut[-x] : -atan_lut[-x];
	}

	return 0;
}


size_t bledecoder::receiver(int16_t* samples, size_t samples_len) {

	feedOne(polar_disc_lut(samples[0], samples[1], pre_r, pre_j));

	for (int i = 2; i < samples_len; i += 2) {

		feedOne(polar_disc_lut(samples[i], samples[i+1], samples[i-2], samples[i-1]));
	}

	pre_r = samples[samples_len - 2];
	pre_j= samples[samples_len - 1];
	return samples_len;
}


bool bledecoder::feedOne(const uint16_t sample) {

	RB_inc();
	RB(0) = (int)sample;

	if (--skipSamples < 1)
	{
		if (DecodePacket(++samples, srate))
		{
			skipSamples = 20;
			return true;
		}
	}
	return false;
}


void bledecoder::RB_inc(void) {
	rb_head++;
	rb_head = (rb_head) % RB_SIZE;
}

inline bool bledecoder::Quantize(int16_t l) {
	return RB(l*g_srate) > g_threshold;
}

static const unsigned char BitReverseTable256[] =
{
        0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0, 0x10, 0x90, 0x50, 0xD0, 0x30, 0xB0, 0x70, 0xF0,
        0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8, 0x18, 0x98, 0x58, 0xD8, 0x38, 0xB8, 0x78, 0xF8,
        0x04, 0x84, 0x44, 0xC4, 0x24, 0xA4, 0x64, 0xE4, 0x14, 0x94, 0x54, 0xD4, 0x34, 0xB4, 0x74, 0xF4,
        0x0C, 0x8C, 0x4C, 0xCC, 0x2C, 0xAC, 0x6C, 0xEC, 0x1C, 0x9C, 0x5C, 0xDC, 0x3C, 0xBC, 0x7C, 0xFC,
        0x02, 0x82, 0x42, 0xC2, 0x22, 0xA2, 0x62, 0xE2, 0x12, 0x92, 0x52, 0xD2, 0x32, 0xB2, 0x72, 0xF2,
        0x0A, 0x8A, 0x4A, 0xCA, 0x2A, 0xAA, 0x6A, 0xEA, 0x1A, 0x9A, 0x5A, 0xDA, 0x3A, 0xBA, 0x7A, 0xFA,
        0x06, 0x86, 0x46, 0xC6, 0x26, 0xA6, 0x66, 0xE6, 0x16, 0x96, 0x56, 0xD6, 0x36, 0xB6, 0x76, 0xF6,
        0x0E, 0x8E, 0x4E, 0xCE, 0x2E, 0xAE, 0x6E, 0xEE, 0x1E, 0x9E, 0x5E, 0xDE, 0x3E, 0xBE, 0x7E, 0xFE,
        0x01, 0x81, 0x41, 0xC1, 0x21, 0xA1, 0x61, 0xE1, 0x11, 0x91, 0x51, 0xD1, 0x31, 0xB1, 0x71, 0xF1,
        0x09, 0x89, 0x49, 0xC9, 0x29, 0xA9, 0x69, 0xE9, 0x19, 0x99, 0x59, 0xD9, 0x39, 0xB9, 0x79, 0xF9,
        0x05, 0x85, 0x45, 0xC5, 0x25, 0xA5, 0x65, 0xE5, 0x15, 0x95, 0x55, 0xD5, 0x35, 0xB5, 0x75, 0xF5,
        0x0D, 0x8D, 0x4D, 0xCD, 0x2D, 0xAD, 0x6D, 0xED, 0x1D, 0x9D, 0x5D, 0xDD, 0x3D, 0xBD, 0x7D, 0xFD,
        0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3, 0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3,
        0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB, 0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB,
        0x07, 0x87, 0x47, 0xC7, 0x27, 0xA7, 0x67, 0xE7, 0x17, 0x97, 0x57, 0xD7, 0x37, 0xB7, 0x77, 0xF7,
        0x0F, 0x8F, 0x4F, 0xCF, 0x2F, 0xAF, 0x6F, 0xEF, 0x1F, 0x9F, 0x5F, 0xDF, 0x3F, 0xBF, 0x7F, 0xFF
};

uint8_t bledecoder::SwapBits(uint8_t a) {

	return BitReverseTable256[a];
//	return (uint8_t)(((a * 0x0802LU & 0x22110LU) | (a * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16);
}


bool bledecoder::DecodePacket(int32_t sample, int srate) {
	bool packet_detected = false;
	g_srate = srate;
	g_threshold = ExtractThreshold();

	if (DetectPreamble()) {

		packet_detected |= DecodeBTLEPacket(sample, srate);

	}
	return packet_detected;
}


int32_t bledecoder::ExtractThreshold(void) {
	int32_t threshold = 0;
	int c;
	for (c = 0; c < 8 * g_srate; c++) {
		threshold += (int32_t)RB(c);
	}
	return (int32_t)threshold / (8 * g_srate);
}

bool bledecoder::DetectPreamble(void) {
	int transitions = 0;
	int c;

	/* preamble sequence is based on the 9th symbol (either 0x55555555 or 0xAAAAAAAA) */
	if (Q(9)) {
		for (c = 0; c < 8; c++) {
			transitions += Q(c) > Q(c + 1);
		}
	}
	else {
		for (c = 0; c < 8; c++) {
			transitions += Q(c) < Q(c + 1);
		}
	}
	return transitions == 4 && abs(g_threshold) < 15500;
}

void bledecoder::ExtractBytes(int l, uint8_t* buffer, int count) {
	int t;
	for (t = 0; t < count; t++) {
		buffer[t] = ExtractByte(l + t * 8);
	}
}


uint8_t bledecoder::ExtractByte(int l) {
	uint8_t byte = 0;
	int c;
	for (c = 0; c < 8; c++) byte |= Q(l + c) << (7 - c);
	return byte;
}

void bledecoder::btle_reverse_whiten(uint8_t chan, uint8_t* data, uint8_t len) {

	uint8_t  i;
	uint8_t lfsr = SwapBits(chan) | 2;
	while (len--) {
		for (i = 0x80; i; i >>= 1) {

			if (lfsr & 0x80) {

				lfsr ^= 0x11;
				(*data) ^= i;
			}
			lfsr <<= 1;
		}
		data++;
	}
}

uint32_t bledecoder::btle_reverse_crc(const uint8_t* data, uint8_t len, uint8_t* dst) {

	uint8_t v, t, d;
	uint32_t crc = 0;
	while (len--) {

		d = SwapBits(*data++);
		for (v = 0; v < 8; v++, d >>= 1) {

			t = dst[0] >> 7;

			dst[0] <<= 1;
			if (dst[1] & 0x80) dst[0] |= 1;
			dst[1] <<= 1;
			if (dst[2] & 0x80) dst[1] |= 1;
			dst[2] <<= 1;


			if (t != (d & 1)) {

				dst[2] ^= 0x5B;
				dst[1] ^= 0x06;
			}
		}
	}
	for (v = 0; v < 3; v++) crc = (crc << 8) | dst[v];
	return crc;
}

uint8_t bledecoder::get_channel_k(uint8_t ch){
	uint8_t rf_channel;
	if(ch==37){
		rf_channel=0;
	}else if(ch==38){
		rf_channel=12;
	}else if(ch==39){
		rf_channel=39;
	}else if(ch >= 0 && ch <= 10){
		rf_channel=ch+1;
	}else if (ch >= 11 && ch<=36){
		rf_channel=ch+2;
	}else{
		rf_channel=0;
	}
	return rf_channel;
}


bool bledecoder::DecodeBTLEPacket(int32_t sample, int srate) {
	int c;
	//struct timeval tv;
	uint8_t packet_data[500];
	int packet_length;
	uint32_t packet_crc;
	uint32_t calced_crc;
	uint64_t packet_addr_l;
	uint32_t packet_addr;
	uint8_t crc[3];
	uint8_t packet_header_arr[2];

	g_srate = srate;

	/* extract address */
	packet_addr_l = 0;
	for (c = 0; c < 4; c++) packet_addr_l |= ((uint64_t)SwapBits(ExtractByte((c + 1) * 8))) << (8 * c);


	/* extract pdu header */
	ExtractBytes(5 * 8, packet_header_arr, 2);

	/* whiten header only so we can extract pdu length */
	btle_reverse_whiten(chan, packet_header_arr, 2);

	if (packet_addr_l == access_addr) {  // Advertisement packet

		packet_length = SwapBits(packet_header_arr[1]) & 0x3F;

		if (packet_length < 2) {
			return false;
		}

	}
	else {
		return false;
		//packet_length = 0;			// TODO: data packets unsupported
	}

	/* extract and whiten pdu+crc */
	ExtractBytes(5 * 8, packet_data, packet_length + 2 + 3);
	btle_reverse_whiten(chan, packet_data, packet_length + 2 + 3);

	if (packet_addr_l == LE_ADV_AA) {  // Advertisement packet
		packet_addr = LE_ADV_AA;
		crc[0] = crc[1] = crc[2] = 0x55;

	}
	else {
		crc[0] = crc[1] = crc[2] = 0;		// TODO: data packets unsupported
	}

	/* calculate packet crc */

	calced_crc = btle_reverse_crc(packet_data, packet_length + 2, crc);

	packet_crc = 0;
	for (c = 0; c < 3; c++) packet_crc = (packet_crc << 8) | packet_data[packet_length + 2 + c];

	/* BTLE packet found, dump information */
	if (packet_crc == calced_crc) {

		int i = 0;
		lell_packet packet;

		packet.access_address = packet_addr;// Advertisement packet
		packet.channel_idx = chan;
		packet.channel_k=get_channel_k(chan);
		packet.adv_type = packet_data[0] & 0xf;
		packet.adv_tx_add = packet_data[0] & 0x40 ? 1 : 0;
		packet.adv_rx_add = packet_data[0] & 0x80 ? 1 : 0;
		packet.flags.as_bits.access_address_ok = (packet.access_address == 0x8e89bed6);//TODO
		packet.access_address_offenses = 0;//TODO

		packet.symbols[0] = packet_addr ;
		packet.symbols[1] = packet_addr >>8;
		packet.symbols[2] = packet_addr >>16;
		packet.symbols[3] = packet_addr >>24;

		packet.length = packet_length;

		for (i = 0; i < packet_length+2+3; i++) {
			packet.symbols[i + 4] = (SwapBits(packet_data[i]));
		}

		callback(packet);
		return true;
	}
	else return false;
}
